#pragma config(Sensor, in1,    rightLine,      sensorLineFollower)
#pragma config(Sensor, in2,    middleLine,     sensorLineFollower)
#pragma config(Sensor, in3,    leftLine,       sensorLineFollower)
#pragma config(Sensor, in4,    lightSensor,    sensorReflection)
#pragma config(Sensor, dgtl2,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl4,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  clawEncoder,    sensorQuadEncoder)
#pragma config(Motor,  port2,           frontLeft,     tmotorVex269, openLoop)
#pragma config(Motor,  port3,           frontRight,    tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           backLeft,      tmotorVex269, openLoop)
#pragma config(Motor,  port5,           backRight,     tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           clawmotor,     tmotorVex269, openLoop)
#pragma config(Motor,  port7,           clawservo,     tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

bool taskActions[3]={false,false,false};
bool taskNotFound = true;
bool endOfLine = false;
int angleFacing;
float distanceTraveled;
int barcodeArray[5] = {-1,-1,-1,-1,-1};

//*********************************************MOVEMENT*************************************************
//written by Francis
  //uses the distance parameters for the distance of the left and right treads, the power parameters to show the motor power, and
  //the forward bools for direction (true spins forward, false spins backwards)
void wait(float time){
  motor[frontLeft]=0;
  motor[backLeft]=0;
  motor[frontRight]=0;
  motor[backRight]=0;
  wait1Msec(time*1000);
}

void moveWheels(float rightDistance, float leftDistance, int rightPower, int leftPower, bool rightForward, bool leftForward){



  motor[frontLeft]=leftPower*(leftForward?1:-1);
  motor[backLeft]=leftPower*(leftForward?1:-1);
  motor[frontRight]=rightPower*(rightForward?1:-1);
  motor[backRight]=rightPower*(rightForward?1:-1);
  SensorValue(leftEncoder) = 0;
  SensorValue(rightEncoder) = 0;
  while(abs(SensorValue(leftEncoder))<(leftDistance*360.0/6.28)&&
        abs(SensorValue(rightEncoder))<(rightDistance*360.0/6.28)){}
  motor[frontLeft]=0;
  motor[backLeft]=0;
  motor[frontRight]=0;
  motor[backRight]=0;
}
  //spins the robot in place, the degrees shows how far to turn, and the bool gives the direction (true spins couter-clockwise/left)
  //uses the distance between treads to find the circumfrence of a circle if the robots treads were two tangent lines
void spin(int degrees, bool spinLeft){
  angleFacing += (spinLeft?degrees:-1*degrees);
  float circleCircumference=45.53;
  float distance=degrees*circleCircumference/360;
  moveWheels(distance,distance,35,35,spinLeft,!spinLeft);
  wait(.1);
}

//drives forward
void moveStraight(float distance, bool forward){
  moveWheels(distance,distance,35,35,forward,forward);
  distanceTraveled+=distance;
}
//void turn(int rightDistance, int leftDistance,int rightPower, int leftPower, bool rightForward, bool leftForward){}

//*********************************************CLAW MOVEMENT***************************************************
//written by Ashley
void moveClaw(){
  motor[clawmotor]=-30;
  SensorValue(clawEncoder)=0;
  while(SensorValue(clawEncoder)<712){}
  motor[clawmotor]=0;
}

void openClaw(){
  motor[clawservo]=200;
}

void closeClaw(){
  motor[clawmotor]=0;
}
//*********************************************BARCODE READING*************************************************
//written by Michael
//(0 if carpet, 1 if dark tape, 2 if white tape)
//first 3 samples of code indicate the start of a bar code with 2,1,2
//next sample indicates weather is is a action or navigation code
//next 2 bits indicate where to go or what location it represents
int analyzeProgram(){

  if(barcodeArray[3]==2){
    return 1;
  }
  if(barcodeArray[3]==1){
    if(barcodeArray[4]==2){
      taskNotFound=false;
      return 2;
    }
    if(barcodeArray[4]==1){
      taskNotFound=false;
      return 3;
    }
  }

  for(int i =0; i<5; i++){
    barcodeArray[i]=-1;
  }

  return 0;
}


int analyzeGround(){
  if(SensorValue(lightSensor)>815){
    return 2;
  }
  else if(SensorValue(lightSensor)>720&&SensorValue(lightSensor)<815){
    return 1;
  }
  else{
    return 0;
  }
}

//***************************************************************************************************
//*********************************************TASKS*************************************************
//***************************************************************************************************

//*********************************************SCAN FOR BARCODE*************************************************
task scanForBarcode(){
  bool isActiveReading = false;
  int groundState = -1;
  while(true){

    while(!isActiveReading){
      wait1Msec(2000);
      groundState = analyzeGround();
      //add to array in first 3 spots (remove position zero and shift over if full)
      barcodeArray[0]=barcodeArray[1];
      barcodeArray[1]=barcodeArray[2];
      barcodeArray[2]=groundState;

      if(barcodeArray[0]==2&&barcodeArray[1]==1&&barcodeArray[2]==2){//if start code is found, flip the isActiveReading to true
         isActiveReading = true;
      }

    }
    int counter = 3;
    while(isActiveReading){
      wait1Msec(2000);
       //create counter to keep track of number of elements filled
      //if(SensorValue(rightEncoder)%5==0){}
      //analyze current ground state
      groundState = analyzeGround();
      barcodeArray[counter]=groundState;  //add ground state to array
      counter++;//increment counter

      if(counter >= 5){//see if the barcode has been read
        isActiveReading = false;
      }
    }
    //call analyzeProgram
    if(analyzeProgram()==1){
      taskActions[0]=true;
      taskNotFound=false;
    }
    if(analyzeProgram()==2){
      taskActions[1]=true;
      taskNotFound=false;
    }
    if(analyzeProgram()==3){
      taskActions[2]=true;
      taskNotFound=false;
    }
  }
}


//*********************************************LINE FOLLOW*************************************************
//written by Trevor
task linefollow(){
  while(true){
    if((SensorValue(rightLine))>2930){
      spin(3,false);//turns right towards line
    }
    else if ((sensorValue(leftLine))>2930){
      spin(3,true);//turns left towards line
    }
    else if((SensorValue(middleLine))>2930){
      moveStraight(1,true);
    }
    else if(SensorValue(middleLine)<2700){
      endOfLine = true;
    }
  }
}

task main(){
  StartTask(scanForBarcode);
  while(taskNotfound){}
  StartTask(linefollow);
  while(!endOfLine){}
  StopTask(linefollow);
  if(taskActions[0]==true){//Action A: lower from top shelf to bottom
    wait(1);
    moveStraight(10,true);
    while(!(SensorValue(middleLine)>2930||SensorValue(rightLine)>2930||SensorValue(leftLine)>2930)){
      spin(5,true);
    }
    StartTask(linefollow);
  }
  if(taskActions[1]==true){//Action B: lower from top shelf to bottom
    wait(1);
    moveStraight(10,true);
    StartTask(linefollow);
  }
  if(taskActions[2]==true){//Action C: lower from top shelf to bottom
    wait(1);
    moveStraight(12,true);
    while(!(SensorValue(middleLine)>2930||SensorValue(rightLine)>2930||SensorValue(leftLine)>2930)){
      spin(5,false);
    }
    StartTask(linefollow);
  }

  while(true){}

}
